#include "control.h"

// 创建并返回一个Control对象的指针
std::unique_ptr<ControllerInterface> ControllerInterface::create() {
    return std::make_unique<Control>("can0");
}

/**
 * @brief 控制类的接收命令处理线程
 *
 * 该线程负责循环监听CAN总线上的命令，并调用解析函数进行处理。
 * 停止条件为stop_标志被设置为true。
 *
 * 成员函数 ReceiveCmdHandleThread 不接受任何参数，也没有返回值。
 *
 * 在循环中，使用 can_->ReceiveCommand(&frame) 尝试接收CAN命令。
 * 如果接收到命令，调用 ParseFrame(frame) 进行解析。
 * 为了降低CPU占用率，每次循环之间有5毫秒的睡眠时间。
 */
void Control::ReceiveCmdHandleThread() {
    struct can_frame frame;

    // 当stop_标志为false时，持续监听CAN命令
    while (!stop_) {
        // 尝试接收CAN命令，如果成功接收到，则解析该命令
        if (can_->ReceiveCommand(&frame)) {
            ParseFrame(frame);
        }

        SimpleTime::sleep_msec(1);  // delay 1ms
    }
}

/**
 * @brief 解析CAN帧并更新车辆里程计信息
 *
 * 本函数根据接收到的CAN帧数据计算车辆的行驶轨迹。它主要通过解析帧中的转向角度和线速度数据，
 * 然后利用这些数据和车辆的轴距来更新车辆的里程计信息，包括车辆的位置(x, y)和航向(yaw)。
 *
 * @param frame 包含车辆行驶数据的CAN帧，包括转向角度和线速度
 */
void Control::ParseFrame(const struct can_frame &frame) {
    uint16_t receive_value = 0;
    switch (frame.can_id) {
        case 0x207: {
            // bit4-5: 转向角度
            receive_value     = frame.data[4] | (frame.data[5] << 8);
            float steer_angle = (receive_value / 100.0) * M_PI / 180.;
            if (receive_value & 0x8000) {
                steer_angle = (int16_t(receive_value - 2 ^ 16) / 100.0) * M_PI / 180.;
            }

            // 当角度为0时，赋一个非常小的角度，保证符合圆周运动模型
            // 由于float精度问题，steer_angle不能设置太小，理论上angle越小越接近直线运动
            if (abs(steer_angle) < 1e-6) {
                steer_angle = std::copysign(1e-6, steer_angle);
            }

            // bit6-7: 线速度
            receive_value         = (frame.data[6] | (frame.data[7] << 8));
            float linear_velocity = RPM2LinearVelocity(receive_value);
            if (receive_value & 0x8000) {
                linear_velocity = RPM2LinearVelocity(int16_t(receive_value - 2 ^ 16));
            }

            // 根据转向角度、线速度和轴距计算并更新里程计信息
            // 推导公式如下，其中x0，y0，θ0为车上一时刻位姿，θ为转向角度，b是轴距，
            // x1，y1，θ1为当前时刻位姿
            // x1 = x0 + (b * sin(θ0 + (v * sinθ * Δt) / b)) / tanθ - b * sinθ0 /tanθ;
            // y1 = y0 - (b * cos(θ0 + (v * sinθ * Δt) / b)) / tanθ + b * conθ0 /tanθ;
            // θ1 = θ0 + (v * sinθ * Δt) / b;
            static uint64_t last_time = SimpleTime::since_epoch_nsec();
            odom_.time                = SimpleTime::since_epoch_nsec();
            float dt                  = (odom_.time - last_time) / 1e9;

            odom_.x += (wheelbase_ * sin(odom_.yaw + (linear_velocity * sin(steer_angle) * dt) / wheelbase_)) /
                           tan(steer_angle) -
                       wheelbase_ * sin(odom_.yaw) / tan(steer_angle);
            odom_.y += wheelbase_ * cos(odom_.yaw) / tan(steer_angle) -
                       (wheelbase_ * cos(odom_.yaw + (linear_velocity * sin(steer_angle) * dt) / wheelbase_)) /
                           tan(steer_angle);
            odom_.yaw += (linear_velocity * sin(steer_angle) * dt) / wheelbase_;

            last_time = odom_.time;
            break;
        }

        default:
            std::cerr << "receive error frame" << std::endl;
            break;
    }
}

/**
 * @brief 发送速度命令
 *
 * 该函数接收线速度和角速度数组作为输入，转换为车辆的行驶和转向命令，
 * 并通过CAN总线发送给相应的执行机构。
 *
 * @param linear_velocity 线速度数组，包含x、y、z三个方向的速度
 * @param angular_velocity 角速度数组，包含roll、pitch、yaw三个方向的速度
 */

void Control::sendVelocity(float linear_velocity[3], float angular_velocity[3]) {
    // 对线速度和角速度进行限制，满足车辆运动学模型的限制。
    LimitVelocity(linear_velocity, angular_velocity);

    float linear_velocity_x  = linear_velocity[0];
    float angular_velocity_z = angular_velocity[2];

    // 将线速度转换为轮速(RPM)
    uint16_t wheel_rpm = LinearVelocitytoRPM(fabs(linear_velocity_x));
    if (wheel_rpm > 8000) {
        std::cerr << "wheel_rpm is out of range:" << wheel_rpm << std::endl;
        return;
    }

    // 计算角速度对应的转向角度
    // TODO:单舵机模型运动学解算:https://www.docin.com/p-769836888.html, 后面会有其他的模型
    // ω = (v * sinθ) / b， 其中b是轮距
    // θ = arcsin((ω * b) / v)

    float theta = std::asin((angular_velocity_z * wheelbase_) / linear_velocity_x);
    float angle = theta * 180 / M_PI;

    // // 将角度映射到合适的范围：[-90°, 90°] -> [-9000, 9000]
    // 顺时针转向为正-> 逆时针为正
    int16_t value = -angle * 100;
    if (value < -9000 || value > 9000) {
        std::cerr << "theta_value is out of range:" << value << std::endl;
        return;
    }

    // 输出到底盘的theta值
    uint16_t theta_value = value;
    if (value < 0) {
        theta_value = theta_value + 2 ^ 16;
    }

    // 准备数据包发送命令
    uint8_t data[8] = {0};
    data[0]         = 0x81;
    // 前进后退使能
    bool forward = linear_velocity_x > 0;
    if (forward) {
        data[0] |= (1 << 2);
    } else {
        data[0] |= (1 << 3);
    }
    data[2] = 0x01;
    data[3] = 0x0A;
    data[4] = theta_value & 0xff;
    data[5] = (theta_value >> 8) & 0xff;
    data[6] = wheel_rpm & 0xff;
    data[7] = (wheel_rpm >> 8) & 0xff;
    can_->SendCommand(0x213, (uint8_t *) &data, sizeof(data));

    uint8_t data2[8] = {0};
    can_->SendCommand(0x313, (uint8_t *) &data2, sizeof(data2));
}

std::pair<uint64_t, std::vector<float>> Control::getOdometry() {
    std::vector<float> odometry;
    odometry.emplace_back(odom_.x);
    odometry.emplace_back(odom_.y);
    odometry.emplace_back(odom_.yaw);

    return std::make_pair(odom_.time, odometry);
}

/**
 * 将线速度转换为转速(RPM)
 * @param linear_velocity 线速度
 * @return 转速(RPM)
 */
int16_t Control::LinearVelocitytoRPM(float linear_velocity) {
    // 根据线速度和轮子半径计算转速：n = (v * 60) / (2pi * r)
    int16_t wheel_rpm = (linear_velocity * 60) / (2 * M_PI * wheel_radius_);
    // 考虑到减速比，调整转速
    wheel_rpm *= driver_motor_reduction_rate_;

    return wheel_rpm;
}

/**
 * 将轮子的转速转换为线速度。
 *
 * 此函数的目的是根据轮子的每分钟转数（RPM）计算机器人的线速度。它通过将转速和轮子的半径
 * 以及驱动电机的减速率考虑在内来完成这一转换。
 *
 * @param wheel_rpm 轮子的每分钟转数（RPM）。
 * @return 计算得到的线速度。
 *
 */
float Control::RPM2LinearVelocity(int16_t wheel_rpm) {
    // 将转速转换为每分钟的弧度数，然后计算线速度。
    float linear_velocity = (wheel_rpm * 2 * M_PI * wheel_radius_) / 60.0;
    // 根据驱动电机的减速率调整线速度。
    linear_velocity /= driver_motor_reduction_rate_;

    return linear_velocity;
}

void Control::LimitVelocity(float *linear_velocity, float *angular_velocity) {
    float &linear_velocity_x  = linear_velocity[0];
    float &angular_velocity_z = angular_velocity[2];

    // 计算转弯半径
    float steer_radius = std::abs(linear_velocity_x / angular_velocity_z);
    // 当转向半径小于轴距时，进行特殊处理
    if (steer_radius < wheelbase_) {
        angular_velocity_z = std::copysign(0.5, angular_velocity_z);
        linear_velocity_x  = std::abs(angular_velocity_z * wheelbase_);
    }

    // 限制线速度
    linear_velocity_x = std::min(linear_velocity_x, max_liner_velocity_);

    // 限制角速度
    if (angular_velocity_z > 0) {
        angular_velocity_z = std::min(angular_velocity_z, max_angle_velocity_);
    } else {
        angular_velocity_z = std::max(angular_velocity_z, -max_angle_velocity_);
    }
}
